#ifndef kinova_driver_qt_KINOVA_CONTROLLER_H
#define kinova_driver_qt_KINOVA_CONTROLLER_H

#include <string>
#include <QThread>
#include <QStringListModel>
#include <QUdpSocket>
#include "basicUtils.hpp"
#include <QXmlStreamWriter>
#include <QXmlStreamReader>
#include "../kinova_driver_qt/kinovaStatusHandler.hpp"
#include <QMutex>
#include <QWaitCondition>

namespace kinova_driver_qt {

    enum JOINT_ID
    {
        JOINT_1,
        JOINT_2,
        JOINT_3,
        JOINT_4,
        JOINT_5,
        JOINT_6
    };
    enum FINGER_ID
    {
        FINGER_1,
        FINGER_2,
        FINGER_3
    };

    class responseHandler : public QThread
    {

    };

class kinovaController : public QThread
{
    Q_OBJECT
public:
        kinovaController( QHostAddress core_ipAddress, int kinova_controller_port,
                          QMutex *_mutex, QWaitCondition *_conditionHandler, kinova_status *_kinova_status);

        kinovaController( int kinova_controller_port );
        ~kinovaController();
        void init( QHostAddress core_ipAddress );

        bool               kinova_set_cpos( Cartesian );
        bool               kinova_set_apos( Joints );
        bool               kinova_send_trajectories( QList<Cartesian> trajectories );
        bool               kinova_goto_homePos();
        bool               kinova_set_fingers( Fingers );
        Status             kinova_get_status();
        bool               kinova_stop();
        QString            kinova_ping();
        void               sendCommand(QByteArray byteArray);
        QByteArray         waitForResponse();
        bool               parseResultXml( QByteArray, QString opType );

      /*bool               kinova_set_joint(JOINT_ID joint_id, double value);
        bool               kinova_set_finger(FINGER_ID finger_id, double value);
        QString            getAddElementData(QXmlStreamReader& xml);
      */

Q_SIGNALS:


private:
        kinovaStatusHandler *statusHandler;
        int                 kinovaControllerPort;
        QHostAddress        coreIpAddress;
        QUdpSocket          *udpSocket;

        QWaitCondition      *conditionHandler;
        QMutex              *mutex;

        kinova_status *kinovaStatus;
};

}  // namespace kinova_driver_qt

#endif
